# vectors.S sends all traps here.
.text
.global __switrap
__switrap:
	# Mode: SWI, I: disabled
	
	# link register offset don't change
	# Only valid for SWI
	
	# Store context
	# Beware of stack overflow
	stmfd sp!, {r0-r12, lr}
	
	# Retrieve SWI number - SWI instruction AND NOT (0xff000000)
	LDR r0, [lr, #-4]
	# Mask off top 8 bits
	bic r0, r0, #0xff000000
	
	# Put trap number in stack (T_source)
	stmfd sp!, {r0}
	
	bl __alltraps
	
	b __trapret

.global __irqtrap
__irqtrap:
	# Mode: IRQ, I: disabled
	
	# link register offset
	# Only valid for IRQ/FIQ
	sub r14, r14, #4
	
	# store context
	# Beware of stack overflow
	stmfd sp!, {r0-r12, lr}
	
	# Retrieve interrupt source
	# Put trap number in stack (32 + INT_source)
	.equ INTOFFSET, 0x4A000014
	ldr r0, =INTOFFSET
	ldr r0, [r0]
	add r0, r0, #32
	stmfd sp!, {r0}
	
	bl __alltraps
	
	# clean source pending
	.equ S, 0x4A000014
	ldr r0, =INTOFFSET
	bl irq_clear
	
	b __trapret

# common part of all exceptions, entry
.globl __alltraps
__alltraps:
	# Put spsr (saved state) in trapframe
	mrs r0, spsr
	stmfd sp!, {r0}
	
	# Put in r0 the adress of the stack
	# to pass a pointer to the trapframe as an argument to trap()
	mov r0, sp

	b trap


# common part of all exceptions, exit
.globl __trapret
__trapret:
	# Get rid of spsr, trap number
	# to prepare for the context restoration
	ldmfd sp!,{r0-r1}
	
	# Exiting the exception handler
	# CPSR is automatically restored
	ldmfd sp!,{r0-r12, pc}^
